#include <rclcpp/rclcpp.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <map_manager/utils.hpp>

using point_cloud_xyz = pcl::PointCloud<pcl::PointXYZ>;

class Publish2DMapNode: public rclcpp::Node {
public:
    explicit Publish2DMapNode(): Node("publish_2d_map_node") {
        this->declare_parameter<std::string>("subscribe_topic", "Laser_map");
        this->declare_parameter<std::string>("publish_topic", "Laser_2d_map");
        this->declare_parameter<std::string>("publish_frame_id", "camera_init");
        this->declare_parameter<double>("min_pz", -0.5);
        this->declare_parameter<double>("max_pz", 0.8);
        this->declare_parameter<bool>("reverse", false);  // Reverse the point cloud x,z coordinates if true, since we add fast lio2 to convert x, z coordinates, this is useless now.
        this->declare_parameter<double>("resolution", 0.1);  // Resolution of the occupancy grid
        this->declare_parameter<double>("grid_value_threshold", 2.0);  // Threshold
        this->declare_parameter<double>("publish_hz", 1.0 / 3.0);  // Publish every 3 seconds
        subscribe_topic_ = this->get_parameter("subscribe_topic").as_string();
        publish_topic_ = this->get_parameter("publish_topic").as_string();
        publish_frame_id_ = this->get_parameter("publish_frame_id").as_string();
        min_pz_ = this->get_parameter("min_pz").as_double();
        max_pz_ = this->get_parameter("max_pz").as_double();
        reverse_ = this->get_parameter("reverse").as_bool();
        resolution_ = this->get_parameter("resolution").as_double();
        grid_value_threshold_ = this->get_parameter("grid_value_threshold").as_double();
        publish_hz_ = this->get_parameter("publish_hz").as_double();

        publish_rate_ = std::make_shared<rclcpp::Rate>(publish_hz_);
        map_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(publish_topic_, 1);
        cloud_pub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            subscribe_topic_, 1,
            std::bind(&Publish2DMapNode::cloud_callback, this, std::placeholders::_1)
        );
        RCLCPP_INFO(this->get_logger(), "Publishing 2D map on topic: %s", subscribe_topic_.c_str());
    }
private:
    void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
        if (msg->data.empty()) {
            RCLCPP_WARN(this->get_logger(), "Received empty point cloud message, skipping processing.");
            return;
        }
        point_cloud_xyz::Ptr cloud(new point_cloud_xyz);
        pcl::fromROSMsg(*msg, *cloud);
        cloud = clip_cloud_and_reverse(cloud, min_pz_, max_pz_, reverse_);
        auto grid = build_2d_map(cloud, resolution_, grid_value_threshold_);
        if (grid == nullptr) {
            RCLCPP_WARN(this->get_logger(), "Failed to build 2D map from point cloud.");
            return;
        }
        // RCLCPP_INFO(this->get_logger(), "Publishing occupancy grid map with dimensions: %dx%d", grid->info.width, grid->info.height);
        grid->header.frame_id = publish_frame_id_;
        grid->header.stamp = this->now();
        map_pub_->publish(*grid);
        publish_rate_->sleep();
    }

    std::string subscribe_topic_;
    std::string publish_topic_;
    std::string publish_frame_id_;
    double min_pz_;
    double max_pz_;
    bool reverse_;
    double resolution_;
    double grid_value_threshold_;
    double publish_hz_;
    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_;
    std::shared_ptr<rclcpp::Rate> publish_rate_;
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Publish2DMapNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
